import pickle
import matplotlib.pyplot as plt # on opensuse need zypper install python3-tk
from matplotlib.pyplot import MultipleLocator
import math


def draw(w):
    with open('Chinese', 'rb') as fp:
        data = pickle.load(fp)
    
    filename = 'write.txt'

    ax = plt.gca()
    x_major_locator = MultipleLocator(10)
    y_major_locator = MultipleLocator(10)
    ax.xaxis.set_major_locator(x_major_locator)
    ax.yaxis.set_major_locator(y_major_locator)
    plt.axis([0, 100, 100, 0])
    plt.grid()

    with open(filename, 'w') as fp:
        word = data[w]
        for i in range(len(word)):
            x_axis = []
            y_axis = []
            if len(word[i]) > 3:
                control_points = []
                for j in range(len(word[i])):
                    control_points.append((word[i][j]['x'], word[i][j]['y']))
                if (degree_check(control_points)):
                    points = bezier(control_points)
                else:
                    points = control_points
                print(points)
                fp.write(str(len(points)))
                for j in points:
                    x = j[0] * 0.1
                    y = j[1] * 0.1
                    x_axis.append(x)
                    y_axis.append(y)
                    fp.write(' '+str(y)+' '+str(x))
            else:
                fp.write(str(len(word[i])))
                for j in range(len(word[i])):
                    x = word[i][j]['x'] * 0.1
                    y = word[i][j]['y'] * 0.1
                    x_axis.append(x)
                    y_axis.append(y)
                    fp.write(' '+str(y)+' '+str(x))
            plt.plot(x_axis, y_axis)
            fp.write('\n')
    plt.show()


def draws(words):
    with open('Chinese', 'rb') as fp:
        data = pickle.load(fp)
    
    filename = 'write.txt'

    ax = plt.gca()
    x_major_locator = MultipleLocator(10)
    y_major_locator = MultipleLocator(10)
    ax.xaxis.set_major_locator(x_major_locator)
    ax.yaxis.set_major_locator(y_major_locator)
    plt.axis([0, 100, 100, 0])
    plt.grid()

    with open(filename, 'w') as fp:
        for k in range(len(words)):
            word = data[words[k]]
            for i in range(len(word)):
                x_axis = []
                y_axis = []
                fp.write(str(len(word[i])))
                for j in range(len(word[i])):
                    x = (word[i][j]['x'] * 0.01) + k * 10
                    y = (word[i][j]['y'] * 0.01)
                    x_axis.append(x)
                    y_axis.append(y)
                    fp.write(' '+str(y)+' '+str(x))
                plt.plot(x_axis, y_axis)
                fp.write('\n')
    plt.show()


def degree_check(control_points):
    for i in range(len(control_points) - 2):
        p1 = control_points[i]
        p2 = control_points[i + 1]
        p3 = control_points[i + 2]
        x = p1[0] - p2[0]
        y = p1[1] - p2[1]
        xx = p3[0] - p2[0]
        yy = p3[1] - p2[1]
        j = (x*x+y*y) + (xx*xx+yy*yy) - ((xx-x)*(xx-x)+(yy-y)*(yy-y))
        j /= 2.0*math.sqrt(x*x+y*y)*math.sqrt(xx*xx+yy*yy)
        if math.acos(j)*180.0/3.14159 < 130:
            return 0
    return 1


def bezier(control_points):
    step = 10
    new_points = []
    for tt in range(step + 1):
        t = tt / step
        tmp_points = control_points
        while len(tmp_points) > 1:
            tmp = []
            for i in range(len(tmp_points) - 1):
                tmp.append((tmp_points[i][0] + t * (tmp_points[i+1][0] - tmp_points[i][0]), tmp_points[i][1] + t * (tmp_points[i+1][1] - tmp_points[i][1])))
            tmp_points = tmp
        new_points.append(tmp_points[0])
    return new_points


def draw_in_word(word):
    ax = plt.gca()
    x_major_locator = MultipleLocator(10)
    y_major_locator = MultipleLocator(10)
    ax.xaxis.set_major_locator(x_major_locator)
    ax.yaxis.set_major_locator(y_major_locator)
    plt.axis([0, 20, 20, 0])
    plt.grid()

    with open('write.txt', 'w') as fp:
        for i in range(len(word)):
            x_axis = []
            y_axis = []
            fp.write(str(len(word[i])))
            for j in range(len(word[i])):
                x = word[i][j]['x']
                y = word[i][j]['y']
                x_axis.append(y)
                y_axis.append(x)
                fp.write(' '+str(y)+' '+str(x))
            plt.plot(x_axis, y_axis)
            fp.write('\n')
    plt.show()


#读取轨迹并画图
def read_and_draw(filename):
    #读取轨迹并转换为list
    with open(filename) as fp:
        file_content = fp.read()
    data = file_content.split(' ')
    word = []
    strokeNum = int(data[0])
    fp = 1
    for i in range(strokeNum):
        stroke = []
        pointNum = int(data[fp])
        for j in range(pointNum):
            point = {}
            point['x'] = float(data[fp+1+j*2])
            point['y'] = float(data[fp+2+j*2])
            stroke.append(point)
        word.append(stroke)
        fp += pointNum*2 + 1
    draw_in_word(word)


def read_actualpath_and_draw(filename):
    xstart = -479.639809
    ystart = -43.596451
    with open(filename) as fp:
        data = fp.readlines()
    x = []
    y = []
    for line in data:
        points = line.split(' ')
        x.append(float(points[0])*1000 - xstart)
        y.append(float(points[1])*1000 - ystart)

    ax = plt.gca()
    plt.axis([0, 20, 20, 0])
    plt.grid()
    
    plt.scatter(x, y)
    
    plt.show()
    

if __name__ == '__main__':
    # draw('能')
    # draws('智能控制')
    # read_and_draw("path.txt")
    read_actualpath_and_draw('actual_move_path.txt')